#ifndef _CHARACTERCONTROLLER_H_
#define _CHARACTERCONTROLLER_H_

//#include <fstream>

#include "XnTypes.h"
#include "Kinect.h"
#include "BoneData.h"
#include "PCCharacterController.h"

using namespace Ogre;

class CharacterController: public PCCharacterController
{
public:

	double m_SmoothingFactor;
	int m_SmoothingDelta;

	bool m_front;	
	bool suppress;

	Ogre::Vector3 m_origTorsoPos;

	CharacterController(Camera* cam):PCCharacterController(cam)
	{
		setupAnimations();

		for(int i = 0; i < 11; i++)
		{
			bones[i].InitialOrientation = mBodyEnt->getSkeleton()->getBone(bones[i].BoneName)->getInitialOrientation();
			bones[i].BonePosition = mBodyEnt->getSkeleton()->getBone(bones[i].BoneName)->getInitialPosition();
		}
		bones[2].InitialOrientation = bones[1].InitialOrientation;


		std::ofstream Initialout("IntialOrientation.txt");
		for(int i = 0; i < 11; i++) {
			//			Initialout << bones[i].BonePosition.x << " " << bones[i].BonePosition.y << " " << bones[i].BonePosition.z << " " 
			//				<< bones[i].InitialOrientation.w << " " << bones[i].InitialOrientation.x << " " << bones[i].InitialOrientation.y << " " << bones[i].InitialOrientation.z << "\n" ;
			Ogre::Vector3 v3 = bones[i].InitialOrientation * Ogre::Vector3::UNIT_Z;
			Initialout << v3 <<"\n";
		}
		Initialout.close();


	}

	void setupBone(const String& name,const Ogre::Radian& angle, const Ogre::Vector3 axis)
	{

		Quaternion q;
		q.FromAngleAxis(angle,axis);	 
		setupBone(name, q);

	}

	void setupBone(const String& name,const Degree& yaw,const Degree& pitch,const Degree& roll)
	{
		Ogre::Bone* bone = mBodyEnt->getSkeleton()->getBone(name);
		bone->setManuallyControlled(true);
		bone->setInheritOrientation(false);

		bone->resetOrientation();

		bone->yaw(yaw);
		bone->pitch(pitch);
		bone->roll(roll);

		//Matrix3 mat = bone->getLocalAxes();
		bone->setInitialState();

	}

	void setupBone(const String& name,const Ogre::Quaternion& q)
	{
		Ogre::Bone* bone = mBodyEnt->getSkeleton()->getBone(name);
		bone->setManuallyControlled(true);
		bone->setInheritOrientation(false);

		bone->resetOrientation();
		bone->setOrientation(q);

		bone->setInitialState();
	}

	void setupAnimations()
	{
		// this is very important due to the nature of the exported animations
		mBodyEnt->getSkeleton()->setBlendMode(ANIMBLEND_CUMULATIVE);	

		String animNames[] =
		{"IdleBase", "IdleTop", "RunBase", "RunTop", "HandsClosed", "HandsRelaxed", "DrawSwords",
		"SliceVertical", "SliceHorizontal", "Dance", "JumpStart", "JumpLoop", "JumpEnd"};

		//set all to manualy controlled
		//Ogre::Bone* handleLeft = mBodyEnt->getSkeleton()->getBone("Hand.L");
		//handleLeft->setManuallyControlled(true);

		//Ogre::Matrix3 mat;
		Ogre::Quaternion q = Quaternion::IDENTITY;
		Quaternion q2,q3;
		Ogre::Vector3 xAxis,yAxis,zAxis;
		q.FromAngleAxis(Ogre::Degree(90),Ogre::Vector3(0,0,-1));
		q.ToAxes(xAxis,yAxis,zAxis);
		q2.FromAngleAxis(Ogre::Degree(90),xAxis);
		setupBone("Humerus.L",q*q2);
		q.FromAngleAxis(Ogre::Degree(90),Ogre::Vector3(0,0,1));
		q.ToAxes(xAxis,yAxis,zAxis);
		q2.FromAngleAxis(Ogre::Degree(90),xAxis);
		setupBone("Humerus.R",q*q2);

		q.FromAngleAxis(Ogre::Degree(90),Ogre::Vector3(0,0,-1));	 
		q2.FromAngleAxis(Ogre::Degree(45),Ogre::Vector3(0,-1,0));

		setupBone("Ulna.L",q*q2);

		q.FromAngleAxis(Ogre::Degree(90),Ogre::Vector3(0,0,1));	 	
		setupBone("Ulna.R",q*q2.Inverse());

		q.FromAngleAxis(Ogre::Degree(180),Ogre::Vector3(0,1,0));
		setupBone("Chest",q);
		setupBone("Stomach",q);
		q.FromAngleAxis(Ogre::Degree(180),Ogre::Vector3(1,0,0));	 	
		q2.FromAngleAxis(Ogre::Degree(180),Ogre::Vector3(0,1,0));
		setupBone("Thigh.L",q*q2);
		setupBone("Thigh.R",q*q2);
		setupBone("Calf.L",q*q2);
		setupBone("Calf.R",q*q2);
		setupBone("Root",Degree(0),Degree(0),Degree(0));

		//rotate the body
		//mBodyNode->yaw(Degree(0),Node::TransformSpace::TS_WORLD);

		Skeleton* skel = mBodyEnt->getSkeleton();

		Ogre::Skeleton::BoneIterator bi = skel->getBoneIterator();

		// populate our animation list
		for (int i = 0; i < NUM_ANIMS; i++)
		{
			mAnims[i] = mBodyEnt->getAnimationState(animNames[i]);
			mAnims[i]->setLoop(true);
			mFadingIn[i] = false;
			mFadingOut[i] = false;

			// disable animation updates
			Animation* anim = mBodyEnt->getSkeleton()->getAnimation(animNames[i]);

			anim->destroyNodeTrack(mBodyEnt->getSkeleton()->getBone("Ulna.L")->getHandle());
			anim->destroyNodeTrack(mBodyEnt->getSkeleton()->getBone("Ulna.R")->getHandle());
			anim->destroyNodeTrack(mBodyEnt->getSkeleton()->getBone("Humerus.L")->getHandle());
			anim->destroyNodeTrack(mBodyEnt->getSkeleton()->getBone("Humerus.R")->getHandle());
			anim->destroyNodeTrack(mBodyEnt->getSkeleton()->getBone("Stomach")->getHandle());
			anim->destroyNodeTrack(mBodyEnt->getSkeleton()->getBone("Chest")->getHandle());
			anim->destroyNodeTrack(mBodyEnt->getSkeleton()->getBone("Thigh.L")->getHandle());
			anim->destroyNodeTrack(mBodyEnt->getSkeleton()->getBone("Thigh.R")->getHandle());
			anim->destroyNodeTrack(mBodyEnt->getSkeleton()->getBone("Calf.L")->getHandle());
			anim->destroyNodeTrack(mBodyEnt->getSkeleton()->getBone("Calf.R")->getHandle());
			anim->destroyNodeTrack(mBodyEnt->getSkeleton()->getBone("Root")->getHandle());
		}

		// start off in the idle state (top and bottom together)
		setBaseAnimation(ANIM_IDLE_BASE);
		setTopAnimation(ANIM_IDLE_TOP);

		// relax the hands since we're not holding anything
		mAnims[ANIM_HANDS_RELAXED]->setEnabled(true);

		mSwordsDrawn = false;
	}

	void resetBonesToInitialState()
	{
		Skeleton* skel = mBodyEnt->getSkeleton();
		skel->getBone("Ulna.L")->resetToInitialState();
		skel->getBone("Ulna.R")->resetToInitialState();
		skel->getBone("Humerus.L")->resetToInitialState();
		skel->getBone("Humerus.R")->resetToInitialState();
		skel->getBone("Stomach")->resetToInitialState();
		skel->getBone("Chest")->resetToInitialState();
		skel->getBone("Thigh.L")->resetToInitialState();
		skel->getBone("Thigh.R")->resetToInitialState();
		skel->getBone("Calf.L")->resetToInitialState();
		skel->getBone("Calf.R")->resetToInitialState();
		skel->getBone("Root")->resetToInitialState();
	}

	void transformBone(const Ogre::String& modelBoneName, XnSkeletonJoint skelJoint, bool flip=false)
	{
		// Get the model skeleton bone info
		Skeleton* skel = mBodyEnt->getSkeleton();
		Ogre::Bone* bone = skel->getBone(modelBoneName);
		Ogre::Quaternion qI = bone->getInitialOrientation();
		Ogre::Quaternion newQ = Kinect::getSingleton().getJointOrientation(skelJoint);

		if(newQ != Ogre::Quaternion::IDENTITY)
		{
			bone->resetOrientation(); //in order for the conversion from world to local to work.
			newQ = bone->convertWorldToLocalOrientation(newQ);

			bone->setOrientation(newQ*qI);	
		}
	}

	void PSupdateBody(Real deltaTime)
	{
		static bool bNewUser = true;
		static bool bRightAfterSwardsPositionChanged = false;
		static Ogre::Vector3 origTorsoPos;

		mGoalDirection = Ogre::Vector3::ZERO;   // we will calculate this
		xn::SkeletonCapability pUserSkel = Kinect::getSingleton().m_UserGenerator.GetSkeletonCap();

		//set smoothing according to the players request.
		if(m_SmoothingDelta!=0)
		{
			m_SmoothingFactor += 0.01 * m_SmoothingDelta;
			if(m_SmoothingFactor >= 1)
				m_SmoothingFactor = 0.99;
			if(m_SmoothingFactor <= 0)
				m_SmoothingFactor = 0.00;
			pUserSkel.SetSmoothing(m_SmoothingFactor);
			Ogre::DisplayString blah = "H/N ";
			blah.append(Ogre::StringConverter::toString((Real)m_SmoothingFactor));
			//m_help->setParamValue("Smoothing", blah);
		}

		// check for start/end pose
		if (IN_POSE_FOR_LONG_TIME == Kinect::getSingleton().m_pEndPoseDetector->checkPoseDuration())
		{
			Kinect::getSingleton().m_UserGenerator.GetSkeletonCap().StopTracking(Kinect::getSingleton().m_candidateID);
			Kinect::getSingleton().m_UserGenerator.GetPoseDetectionCap().StartPoseDetection("Psi", Kinect::getSingleton().m_candidateID);
			Kinect::getSingleton().m_candidateID = 0;
			resetBonesToInitialState();
			Kinect::getSingleton().m_pEndPoseDetector->Reset();
			Kinect::getSingleton().m_pStartPoseDetector->Reset();

			// end pose OK - re-apply the quit slider
			// but make sure we're not in a session already -- nuke the session generator
			Kinect::getSingleton().m_pSessionManager->EndSession();
			Kinect::getSingleton().m_pQuitFlow->SetActive(Kinect::getSingleton().m_pQuitSSlider);

			suppress = false;
			bNewUser = true;

			return;
		}

		// We dont care about the result of this, it is a simple progress-keeping mechanism
		Kinect::getSingleton().m_pStartPoseDetector->checkPoseDuration();

		Skeleton* skel = mBodyEnt->getSkeleton();
		Ogre::Bone* rootBone = skel->getBone("Root");

		XnSkeletonJointPosition torsoPos;

		if (pUserSkel.IsTracking(Kinect::getSingleton().m_candidateID))
		{
			if(bNewUser)
			{			
				pUserSkel.GetSkeletonJointPosition(Kinect::getSingleton().m_candidateID, XN_SKEL_TORSO, torsoPos);
				if(torsoPos.fConfidence > 0.5)
				{
					origTorsoPos.x = -torsoPos.position.X;
					origTorsoPos.y = torsoPos.position.Y;
					origTorsoPos.z = -torsoPos.position.Z;
					bNewUser = false;
				}
			}

			transformBone("Stomach",XN_SKEL_TORSO, true);
			transformBone("Waist", XN_SKEL_WAIST);
			transformBone("Root", XN_SKEL_WAIST);
			transformBone("Chest",XN_SKEL_TORSO, true);
			transformBone("Humerus.L",XN_SKEL_LEFT_SHOULDER);
			transformBone("Humerus.R",XN_SKEL_RIGHT_SHOULDER);
			transformBone("Ulna.L",XN_SKEL_LEFT_ELBOW);
			transformBone("Ulna.R",XN_SKEL_RIGHT_ELBOW);
			transformBone("Thigh.L",XN_SKEL_LEFT_HIP);
			transformBone("Thigh.R",XN_SKEL_RIGHT_HIP);
			transformBone("Calf.L",XN_SKEL_LEFT_KNEE);
			transformBone("Calf.R",XN_SKEL_RIGHT_KNEE);

			if(!bNewUser)
			{			 
				pUserSkel.GetSkeletonJointPosition(Kinect::getSingleton().m_candidateID, XN_SKEL_TORSO, torsoPos);
				Ogre::Vector3 newPos;
				newPos.x = -torsoPos.position.X;
				newPos.y = torsoPos.position.Y;
				newPos.z = -torsoPos.position.Z;

				Ogre::Vector3 newPos2 = (newPos - origTorsoPos)/100;

				newPos2.y -= 0.3;

				if (newPos2.y < 0)
				{
					newPos2.y /= 2.5;

					if (newPos2.y < -1.5)
					{
						newPos2.y = -1.5;
					}
				}
				if(torsoPos.fConfidence > 0.5)
				{
					rootBone->setPosition(newPos2);
				}
			}

			//do gestures for swards
			if ((mTopAnimID == ANIM_IDLE_TOP || mTopAnimID == ANIM_RUN_TOP))
			{
				XnSkeletonJointPosition headPos;
				XnSkeletonJointPosition leftHand;
				XnSkeletonJointPosition rightHand;
				pUserSkel.GetSkeletonJointPosition(Kinect::getSingleton().m_candidateID, XN_SKEL_HEAD, headPos);
				pUserSkel.GetSkeletonJointPosition(Kinect::getSingleton().m_candidateID, XN_SKEL_LEFT_HAND, leftHand);
				pUserSkel.GetSkeletonJointPosition(Kinect::getSingleton().m_candidateID, XN_SKEL_RIGHT_HAND, rightHand);
				if(leftHand.fConfidence > 0 && headPos.fConfidence > 0)
				{
					XnV3DVector leftvec = XnV3DVector(leftHand.position);
					XnV3DVector rightvec = XnV3DVector(rightHand.position);
					XnV3DVector headvec = XnV3DVector(headPos.position);
					XnV3DVector tempvec;
					tempvec = leftvec - rightvec;
					if(tempvec.MagnitudeSquared() < 50000)
					{
						tempvec = leftvec - headvec;
						if(!bRightAfterSwardsPositionChanged &&
							tempvec.MagnitudeSquared() < 100000)
						{

							if(leftHand.position.Z+150 > headPos.position.Z)
							{
								// take swords out (or put them back, since it's the same animation but reversed)
								setTopAnimation(ANIM_DRAW_SWORDS, true);
								mTimer = 0;
								bRightAfterSwardsPositionChanged = true;
							}
						}
					} else
					{
						bRightAfterSwardsPositionChanged = false;
					}
				}	
			}
		} // end if player calibrated
		else
		{
			//return to initialState
			if(!bNewUser)
			{
				rootBone->resetToInitialState();
			}
			bNewUser = true;
		}
	}

	void addTime(Real deltaTime)
	{
		Kinect::getSingleton().m_Context.WaitNoneUpdateAll();
		Kinect::getSingleton().m_pSessionManager->Update( &(Kinect::getSingleton().m_Context) );
		this->suppress = Kinect::getSingleton().suppress;
		this->m_front = Kinect::getSingleton().m_front;
		this->m_origTorsoPos = Kinect::getSingleton().TorsoPos;
		this->m_SmoothingDelta = Kinect::getSingleton().m_SmoothingDelta;
		this->m_SmoothingFactor = Kinect::getSingleton().m_SmoothingFactor;
		if(Kinect::getSingleton().isLost)
		{
			this->resetBonesToInitialState();
		}

		updateBody(deltaTime);
		updateAnimations(deltaTime);
		PSupdateBody(deltaTime);
		updateCamera(deltaTime);

		recordBoneData();
	}

};

#endif
